#include "drawinpic.h"

#include "commonfunctionset.h"
#include "eigenfuncset.h"
#include <pcl/common/transforms.h>

DrawInPic::DrawInPic()
{}

DrawInPic::~DrawInPic()
{}

bool DrawInPic::Init(cv::Mat& external, cv::Mat& intrinsic)
{
	m_exteMatrix = external;
	m_intriMatrix = intrinsic;

	return true;
}

void DrawInPic::DrawPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::Scalar sc, cv::Mat& image)
{
	cv::Mat externalMatrixToCam = m_exteMatrix.inv();
	Eigen::Matrix4f tocam = Eigen::Matrix4f::Identity();
	if (!externalMatrixToCam.empty()) {
		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				tocam(i, j) = externalMatrixToCam.at<double>(i, j) / 1000;//double转float
			}
		}
	}
	pcl::PointCloud<pcl::PointXYZ>::Ptr palltCamCloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*cloud, *palltCamCloud, tocam);
	pcl::PointXYZ left_top_point(palltCamCloud->points.at(0).x, palltCamCloud->points.at(0).y, palltCamCloud->points.at(0).z);
	pcl::PointXYZ left_bottom_point(palltCamCloud->points.at(1).x, palltCamCloud->points.at(1).y, palltCamCloud->points.at(1).z);
	pcl::PointXYZ right_bottom_point(palltCamCloud->points.at(2).x, palltCamCloud->points.at(2).y, palltCamCloud->points.at(2).z);
	pcl::PointXYZ right_top_point(palltCamCloud->points.at(3).x, palltCamCloud->points.at(3).y, palltCamCloud->points.at(3).z);

	//4个角点的顺序不对，需要排序后，再计算底部中心点
	//基于相机方向（向下为Y增加，向右为X增加）且空间中箱子基本不倾斜，区分4个角点
	std::vector<pcl::PointXYZ> vPoint = { left_top_point ,left_bottom_point ,right_bottom_point ,right_top_point };
	std::vector<float> vY = { left_top_point.y ,left_bottom_point.y ,right_bottom_point.y ,right_top_point.y };
	std::vector<int> index_by_Y = CommonFun::sort_index(vY, 1);//获取较大的Y值索引  0 1 是Y较大的下面的点  2 3是Y较小的上面的点 
	if (vPoint[index_by_Y[0]].x < vPoint[index_by_Y[1]].x) {
		left_bottom_point = vPoint[index_by_Y[0]];
		right_bottom_point = vPoint[index_by_Y[1]];
	} else {
		left_bottom_point = vPoint[index_by_Y[1]];
		right_bottom_point = vPoint[index_by_Y[0]];
	}
	if (vPoint[index_by_Y[2]].x < vPoint[index_by_Y[3]].x) {
		left_top_point = vPoint[index_by_Y[2]];
		right_top_point = vPoint[index_by_Y[3]];
	} else {
		left_top_point = vPoint[index_by_Y[3]];
		right_top_point = vPoint[index_by_Y[2]];
	}

	if (1) {
		//绘制外接矩形
		std::vector<pcl::PointXYZ> vPoint = { left_top_point ,left_bottom_point ,right_bottom_point ,right_top_point };
		//3D点云数据转化
		std::vector<cv::Point3f> CloudPoints;//空间点
		for (const auto& point : vPoint) {
			{
				CloudPoints.push_back(cv::Point3f(point.x, point.y, point.z));
			}
		}

		//把点投影到2D平面上
		Eigen::Matrix4d CamPose = EigenFunc::TransEulerPostion2Matrix({ 0, 0, 0, 0, 0, 0 });
		cv::Mat CamPoseMat = EigenFunc::MatrixToMat4d(CamPose);
		cv::Mat Rvec, Tvec;
		EigenFunc::HomMat3dToRVT(CamPoseMat, &Rvec, &Tvec);

		std::vector<cv::Point2f> ImagePoints;//绘制箭头投影图像点
		cv::projectPoints(CloudPoints, Rvec, Tvec, m_intriMatrix, m_distorMatrix, ImagePoints);

		std::vector<cv::Point2f> ImagePoints2{ ImagePoints[1],ImagePoints[2] ,ImagePoints[3],ImagePoints[0] };
		//绘制外接矩形
		EigenFunc::lines(image, ImagePoints, ImagePoints2, sc, 3);
	}
}

void DrawInPic::DrawTray(cv::Mat& image)
{

}

void DrawInPic::DrowBox(cv::RotatedRect rect, cv::Mat& image)
{

}
